// #include <Arduino.h>
// #include "NewPing.h"
#include <SoftwareSerial.h>

//蓝牙适配器 HC-05
SoftwareSerial mySerial(3, 2); //HC-05 Tx & Rx is connected to Arduino #3 & #2

void BluetoothInit();
void BluetoothLoop();
void CarInit();
void CarDo(String code);

// Motor A connections
int enA = 6;
int in1 = 7;
int in2 = 8;
// Motor B connections
int in3 = 9;
int in4 = 10;
int enB = 11;

// put function declarations here:
void goForward(int);

void setup() {
  //串口波特率
  Serial.begin(9600);
  
  //车辆初始化
  CarInit();

  //蓝牙初始化
  BluetoothInit();
}

void loop() {
  //蓝牙监听循环
  BluetoothLoop();
}

// 车辆初始化
void CarInit() {
  pinMode(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}

// 前进
void goForward() {
    analogWrite(enA, 250 / 2);
    analogWrite(enB, 250 / 2);
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
}

// 后退
void goBack() {
  analogWrite(enA, 250 / 2);
  analogWrite(enB, 250 / 2);
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
}

// 左转
void goLeft() {
    analogWrite(enA, 75);
    analogWrite(enB, 250);
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
}

// 右转
void goRight() {
    analogWrite(enA, 250);
    analogWrite(enB, 75);
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
}

// 停止车辆
void stopCar() {
  analogWrite(enA, 0);
  analogWrite(enB, 0);
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}

// 车辆控制
void CarDo(String code) {
  // code=left: 左转
  // code=right: 右转
  // code=forward: 向前
  // code=back: 后退
  // code=stop: 停止
  if (code == "left") {
    Serial.println("小车左转");
	mySerial.write("小车左转");
    goLeft();
  }
  else if (code == "right") {
    Serial.println("小车右转");
	mySerial.write("小车右转");
    goRight();
  }
  else if (code == "forward") {
    Serial.println("小车前进");
	mySerial.write("小车前进");
    goForward();
  }
  else if (code == "back") {
    Serial.println("小车后退");
	mySerial.write("小车后退");
    goBack();
  }
  else if (code == "stop") {
    Serial.println("小车停止");
	mySerial.write("小车停止");
    stopCar();
  }
}

// 初始化蓝牙模块
void BluetoothInit() {
	// 开始使用 Arduino 和串行监视器进行串行通信。
	//Serial.begin(9600);
	
	// 开始使用 Arduino 和 HC-05 进行通信。
	mySerial.begin(9600);

	Serial.println("初始化...");
	Serial.println("设备准备就绪，请配对你的蓝牙!");
}

// 蓝牙模块接收和发送数据
void BluetoothLoop() {
	static String receivedString;
	if(Serial.available() > 0) 
	{
		char receivedChar = Serial.read();
		mySerial.write(receivedChar);

		// 检查是否为终止字符
		if (receivedChar == '\n') {
			// 打印完整的字符串
			Serial.print("串口发出的字符串: ");
			Serial.println(receivedString);
			receivedString = "";  // 重置字符串
		} else {
			// 将字符添加到字符串
			receivedString += receivedChar;
		}
	}
	
	static String readString;
	if(mySerial.available() > 0) 
	{
		char readChar = mySerial.read();
		if (readChar == '\n') {

    	// ShowString(readString);

			// 打印完整的字符串
			Serial.print("蓝牙接收的字符串: ");
			Serial.println(readString);
      
            // 车辆控制
            CarDo(readString);
			readString = "";  // 重置字符串
		}
		else if (readChar == '\r') {

		} else {
			// 将字符添加到字符串
			readString += readChar;
		}
	}
	delay(20);
}